#include <stdio.h>

#include <windows.h>
#include <SOLID/solid.h>

#include <3D/Point.h>
#include <3D/Quaternion.h>

#define SPACE_SIZE 20
#define NUM_ITER 10000

#include <string.h>

typedef struct MyObject {
  int id;
} MyObject; 

/* ARGSUSED */
void collide1(DtPointer client_data, DtObjectRef obj1, DtObjectRef obj2,
	      const DtCollData *coll_data) {
}

/* ARGSUSED */
void collide2(DtPointer client_data, DtObjectRef obj1, DtObjectRef obj2,
	      const DtCollData *coll_data) {
  FILE *stream = (FILE *)client_data;
  fprintf(stream, "Object %d interferes with object %d\n", 
	  (*(MyObject *)obj1).id, (*(MyObject *)obj2).id);
}

/* ARGSUSED */
void collide3(DtPointer client_data, DtObjectRef obj1, DtObjectRef obj2,
	      const DtCollData *coll_data) {
  FILE *stream = (FILE *)client_data;
  fprintf(stream, "Smart: Object %d interferes with object %d, nseg %d\n", 
	  (*(MyObject *)obj1).id, (*(MyObject *)obj2).id, coll_data->numverts);
}



int main() {
	MyObject object1, object2;

	object1.id = 1;
	object2.id = 2;

	double a = 10; 
	double b = 2; 

	DWORD start=GetTickCount ();
	DWORD end=0;
	int nruns = 50;
	int timebuildtorus = 0;
	int timebuildtree = 0;
	int timecs = 0;

	for (int w = 0; w < nruns; w++) {
        dtNewShape("torus");
		// printf("Loading a torus with a major radius of %d and a minor radius of %d%,\n", (int)a, (int)b);
		  
		const int n1 = 50;
		const int n2 = 50;
			
		// printf("composed of %d polygons...\n", n1 * n2); 
		start=GetTickCount ();
		for (int uc = 0; uc < n1; uc++)
			for (int vc = 0; vc < n2; vc++) {
				double u1 = (2.0*M_PI*uc) / n1; 
				double u2 = (2.0*M_PI*(uc+1)) / n1; 
				double v1 = (2.0*M_PI*vc) / n2; 
				double v2 = (2.0*M_PI*(vc+1)) / n2; 
			  
				double p1[3], p2[3], p3[3], p4[3];
			  
				p1[0] = (a - b * cos(v1)) * cos(u1);
				p2[0] = (a - b * cos(v1)) * cos(u2);
				p3[0] = (a - b * cos(v2)) * cos(u1);
				p4[0] = (a - b * cos(v2)) * cos(u2);
				p1[1] = (a - b * cos(v1)) * sin(u1);
				p2[1] = (a - b * cos(v1)) * sin(u2);
				p3[1] = (a - b * cos(v2)) * sin(u1);
				p4[1] = (a - b * cos(v2)) * sin(u2);
				p1[2] = b * sin(v1);
				p2[2] = b * sin(v1);
				p3[2] = b * sin(v2);
				p4[2] = b * sin(v2);
			  
				dtNewPolygon();
			  
				dtVertex(p1[0], p1[1], p1[2]);
				dtVertex(p2[0], p2[1], p2[2]);
				dtVertex(p4[0], p4[1], p4[2]);
				dtVertex(p3[0], p3[1], p3[2]);
			  
				dtEndPolygon();
		}
		end  = GetTickCount ();
		timebuildtorus += (end-start);

		// printf("Building hierarchy...\n");
		start=GetTickCount ();
		dtEndShape();
		end  = GetTickCount ();
		timebuildtree += (end-start);
		// printf("done\n");
		  
		dtNewObject(&object1, "torus"); 
		dtNewObject(&object2, "torus"); 

		dtDisableNBody();
		  
		dtSetDefaultResponse(collide1, DT_SIMPLE_RESPONSE, stdout);

		int col_count = 0;
		Quaternion q;
		Quaternion q2;

		FILE *fp;
		char nomefile[13];
		char wstr[3];
        strcpy(nomefile,"ptquat");
        itoa ((w+1),wstr,10);
        strcat(nomefile,wstr);
        strcat(nomefile,".txt");
		fp=fopen(nomefile, "w");
			// printf("Running %d tests with two tori at random placements ", NUM_ITER);
			// printf("in a space of size %d...\n", SPACE_SIZE);  
		for (int i = 0; i != NUM_ITER; ++i) {
			double pos1x = rnd() * SPACE_SIZE;
			double pos1y = rnd() * SPACE_SIZE;
			double pos1z = rnd() * SPACE_SIZE;
			double pos2x = rnd() * SPACE_SIZE;
			double pos2y = rnd() * SPACE_SIZE;
			double pos2z = rnd() * SPACE_SIZE;
			q = Quaternion::random();
			q2 = Quaternion::random();
			fprintf(fp, "%.15f\t%.15f\t%.15f\t%.15f\t%.15f\t%.15f\t%.15f\t",pos1x,pos1y,pos1z,q[X], q[Y], q[Z], q[W]);
			fprintf(fp, "%.15f\t%.15f\t%.15f\t%.15f\t%.15f\t%.15f\t%.15f\n",pos2x,pos2y,pos2z,q2[X], q2[Y], q2[Z], q2[W]);

			start=GetTickCount ();

			dtSelectObject(&object1);
			dtPosition(pos1x, pos1y, pos1z);
			dtOrientation(q[X], q[Y], q[Z], q[W]);
			dtSelectObject(&object2);
			dtPosition(pos2x, pos2y, pos2z);
			dtOrientation(q2[X], q2[Y], q2[Z], q2[W]);
			if (dtTest()) ++col_count;

			end  = GetTickCount ();
			timecs += (end-start);
		}

		// printf("done\n");
		fclose(fp);

		// printf("Number of collisions: %d\n",col_count);
		printf("%d\n",col_count);

		// cout << "Number of collisions: " << col_count << endl;

        // printf("Iterazione %d\n",(w+1));
		
		dtDeleteShape("torus");
	}
	dtDeleteObject(&object1);
	dtDeleteObject(&object2);
	// return 0;
	  
	printf("ELENCO TEMPI (basati su %d iterazioni)\n",nruns);
	printf("Creazione torus (ms): %g\n",(double)timebuildtorus/(double)nruns);
	printf("Creazione gerarchia (ms): %g\n",(double)timebuildtree/(double)nruns);
	printf("Singola collision checking (ms): %g\n",(double)timecs/(double)(NUM_ITER*nruns));
	printf("Tutte le %d collision checking (ms): %d\n",NUM_ITER*nruns,timecs);

	system("PAUSE");
	return EXIT_SUCCESS;
}
